//
// Created by chen on 25-5-23.
//
#include "stm32f1xx_hal.h"  // 必须首先包含 HAL 主头文件
#include "stm32f1xx_hal_tim.h"
#include "FOCtroler.h"
#include "motor_runtime_param.h" // 包含电机实时参数定义（如motor_logic_angle等）
#include <algorithm> // 包含标准库头文件
#include "arm_math.h"
#include <cmath>
#include <cstdio>

#define deg2rad(a) (PI * (a) / 180)
#define rad2deg(a) (180 * (a) / PI)
#define max(a, b) ((a) > (b) ? (a) : (b))
#define min(a, b) ((a) < (b) ? (a) : (b))
#define rad60 deg2rad(60)
#define SQRT3 1.73205080756887729353

extern  TIM_HandleTypeDef htim1;
// 构造函数实现
CFOCController::CFOCController() {
    resetPidParameters();
    //初始化为速度控制
    m_controlParams.mode = ControlMode::kSpeed;

    // 初始化PID实例
    arm_pid_init_f32(&m_pidPosition, false);
    arm_pid_init_f32(&m_pidSpeed, false);
    arm_pid_init_f32(&m_pidTorqueD, false);
    arm_pid_init_f32(&m_pidTorqueQ, false);
}

void CFOCController::resetPidParameters() {
    // 位置环PID参数（默认值）
    m_pidPosition.Kp = 10.0f;  // 位置比例增益
    m_pidPosition.Ki = 0.5f;   // 位置积分增益
    m_pidPosition.Kd = 0.1f;   // 位置微分增益

    // 速度环PID参数（默认值）
    m_pidSpeed.Kp = 2.0f;      // 速度比例增益
    m_pidSpeed.Ki = 0.01f;      // 速度积分增益
    m_pidSpeed.Kd = 0.0f;     // 速度微分增益

    // d轴电流环PID参数（默认值）
    m_pidTorqueD.Kp = 1.5f;    // d轴电流比例增益
    m_pidTorqueD.Ki = 2.0f;    // d轴电流积分增益
    m_pidTorqueD.Kd = 0.0f;    // d轴电流微分增益（通常不需要）

    // q轴电流环PID参数（默认值）
    m_pidTorqueQ.Kp = 1.5f;    // q轴电流比例增益
    m_pidTorqueQ.Ki = 2.0f;    // q轴电流积分增益
    m_pidTorqueQ.Kd = 0.0f;    // q轴电流微分增益（通常不需要）

    // 重置积分项和上一次误差值
    m_pidPosition.state[0] = 0.0f;  // 积分项
    m_pidPosition.state[1] = 0.0f;  // 上一次误差

    m_pidSpeed.state[0] = 0.0f;     // 积分项
    m_pidSpeed.state[1] = 0.0f;     // 上一次误差

    m_pidTorqueD.state[0] = 0.0f;   // 积分项
    m_pidTorqueD.state[1] = 0.0f;   // 上一次误差

    m_pidTorqueQ.state[0] = 0.0f;   // 积分项
    m_pidTorqueQ.state[1] = 0.0f;   // 上一次误差
}

// 角度循环差分计算实现
float CFOCController::cycleDiff(float current, float target, float cycle) const {
    float diff = target - current;
    diff -= cycle * std::floor((diff + cycle/2) / cycle);
    return diff;
}

// SVPWM调制实现
// void CFOCController::svpwm(float theta, float vd, float vq, float& du, float& dv, float& dw) const {
//     vd = std::clamp(vd, -1.0f, 1.0f);
//     vq = std::clamp(vq, -1.0f, 1.0f);
//
//     const int8_t sectorTable[8] = {4, 6, 5, 5, 3, 1, 2, 2};
//     const int8_t vector[6][3] = {{1, 0, 0}, {1, 1, 0}, {0, 1, 0},
//                                  {0, 1, 1}, {0, 0, 1}, {1, 0, 1}};
//
//     float alpha, beta;
//     arm_inv_park_f32(vd, vq, &alpha, &beta, arm_sin_f32(theta), arm_cos_f32(theta));
//
//     bool A = beta > 0;
//     bool B = std::fabs(beta) > kSqrt3 * std::fabs(alpha);
//     bool C = alpha > 0;
//     int8_t K = 4*A + 2*B + C;
//     int8_t sector = sectorTable[K];
//     //printf("sector:%d\n", sector);
//
//     float t1 = arm_sin_f32(sector * kRad60) * alpha -
//                arm_cos_f32(sector * kRad60) * beta;
//     float t2 = beta * arm_cos_f32((sector-1)*kRad60) -
//                alpha * arm_sin_f32((sector-1)*kRad60);
//     float t0 = 1.0f - t1 - t2;
//
//     du = t1 * vector[sector-1][0] + t2 * vector[sector%6][0] + t0/2;
//     dv = t1 * vector[sector-1][1] + t2 * vector[sector%6][1] + t0/2;
//     dw = t1 * vector[sector-1][2] + t2 * vector[sector%6][2] + t0/2;
// }

void CFOCController::svpwm(float phi, float d, float q, float *d_u, float *d_v, float *d_w)
{
    d = min(d, 1);
    d = max(d, -1);
    q = min(q, 1);
    q = max(q, -1);
    const int v[6][3] = {{1, 0, 0}, {1, 1, 0}, {0, 1, 0}, {0, 1, 1}, {0, 0, 1}, {1, 0, 1}};
    const int K_to_sector[] = {4, 6, 5, 5, 3, 1, 2, 2};
    float sin_phi = arm_sin_f32(phi);
    float cos_phi = arm_cos_f32(phi);
    float alpha = 0;
    float beta = 0;
    arm_inv_park_f32(d, q, &alpha, &beta, sin_phi, cos_phi);

    bool A = beta > 0;
    bool B = fabs(beta) > SQRT3 * fabs(alpha);
    bool C = alpha > 0;

    int K = 4 * A + 2 * B + C;
    int sector = K_to_sector[K];

    float t_m = arm_sin_f32(sector * rad60) * alpha - arm_cos_f32(sector * rad60) * beta;
    float t_n = beta * arm_cos_f32(sector * rad60 - rad60) - alpha * arm_sin_f32(sector * rad60 - rad60);
    float t_0 = 1 - t_m - t_n;

    *d_u = t_m * v[sector - 1][0] + t_n * v[(sector) % 6][0] + t_0 / 2;
    *d_v = t_m * v[sector - 1][1] + t_n * v[(sector) % 6][1] + t_0 / 2;
    *d_w = t_m * v[sector - 1][2] + t_n * v[(sector) % 6][2] + t_0 / 2;
}

// 位置环控制实现
float CFOCController::runPositionLoop(float targetRad) const {
    return arm_pid_f32(&m_pidPosition, cycleDiff(motor_logic_angle, targetRad, position_cycle));
}

// 速度环控制实现
float CFOCController::runSpeedLoop(float targetRadPerSec) const {
    float diff=targetRadPerSec - motor_speed;
    return arm_pid_f32(&m_pidSpeed, diff);
}

// d轴电流环控制实现
float CFOCController::runTorqueDLoop(float targetNorm) const {
    float error = targetNorm - (motor_i_d / MAX_CURRENT);
    float output = arm_pid_f32(&m_pidTorqueD, error);
    output = std::clamp(output, -1.0f, 1.0f);
    handleIntegralWindup(m_pidTorqueD, error, output);
    return output;
}

// q轴电流环控制实现
float CFOCController::runTorqueQLoop(float targetNorm) const {
    float error = targetNorm - (motor_i_q / MAX_CURRENT);
    float output = arm_pid_f32(&m_pidTorqueQ, error);
    output = std::clamp(output, -1.0f, 1.0f);
    handleIntegralWindup(m_pidTorqueQ, error, output);
    return output;
}

// 积分饱和处理实现
/**
 * 处理PID控制器的积分饱和问题
 * @param pid 引用PID控制器实例
 * @param error 当前误差值
 * @param output 计算得到的PID输出值
 */
void CFOCController::handleIntegralWindup(arm_pid_instance_f32& pid, float error, float output) const {
    // 计算理论PID输出（不考虑积分饱和）
    float theoreticalOutput = pid.Kp * error +
                              pid.Ki * pid.state[0] +
                              pid.Kd * (error - pid.state[1]);

    // 计算积分饱和量（即实际输出与理论输出的差值）
    float windup = output - theoreticalOutput;

    // 应用积分抗饱和（通过调整积分项state[0]）
    // 乘以0.7是为了减小积分项的调整幅度，避免系统震荡
    pid.state[0] -= pid.Ki * 0.7f * windup;
}

void CFOCController::applyPwm(float du, float dv, float dw) {
    __disable_irq();
    uint32_t arr = htim1.Instance->ARR;
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, du * arr);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, dv * arr);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, dw * arr);
    __enable_irq();
}

// 设置PID参数实现
void CFOCController::setPidGains(
    float posKp, float posKi, float posKd,
    float spdKp, float spdKi, float spdKd,
    float tdKp, float tdKi, float tdKd,
    float tqKp, float tqKi, float tqKd)
{
    m_pidPosition.Kp = posKp;
    m_pidPosition.Ki = posKi;
    m_pidPosition.Kd = posKd;

    m_pidSpeed.Kp = spdKp;
    m_pidSpeed.Ki = spdKi;
    m_pidSpeed.Kd = spdKd;

    m_pidTorqueD.Kp = tdKp;
    m_pidTorqueD.Ki = tdKi;
    m_pidTorqueD.Kd = tdKd;

    m_pidTorqueQ.Kp = tqKp;
    m_pidTorqueQ.Ki = tqKi;
    m_pidTorqueQ.Kd = tqKd;
}

// 控制模式接口实现

void CFOCController::setPositionControl(float targetRad) {
    m_controlParams = {targetRad, 0, 0, 0, 0, 0, ControlMode::kPosition};
}

void CFOCController::setSpeedControl(float targetRadPerSec) {
    m_controlParams = {0, targetRadPerSec, 0, 0, 0, 0, ControlMode::kSpeed};
}

void CFOCController::setTorqueControl(float dNorm, float qNorm) {
    m_controlParams = {0, 0, dNorm, qNorm, 0, 0, ControlMode::kTorque};
}

void CFOCController::setSpeedTorqueControl(float speedRadPerSec, float maxTorqueNorm) {
    m_controlParams = {0, speedRadPerSec, 0, 0, 0, maxTorqueNorm, ControlMode::kSpeedTorque};
}

void CFOCController::setPositionSpeedTorqueControl(float posRad, float maxSpeed, float maxTorque) {
    m_controlParams = {posRad, 0, 0, 0, maxSpeed, maxTorque, ControlMode::kPositionSpeedTorque};
}

// 执行控制周期实现
void CFOCController::update(float e_angle) {
    if (m_controlParams.mode == ControlMode::kNull) return;

    float d = 0.0f, q = 0.0f;
    float rotorAngle = e_angle; // 假设来自硬件反馈

    switch (m_controlParams.mode) {
        case ControlMode::kPosition:
            q = runPositionLoop(m_controlParams.position);
            break;
        case ControlMode::kSpeed:
            //q = runSpeedLoop(m_controlParams.speed);
            break;
        case ControlMode::kTorque:
            d = runTorqueDLoop(m_controlParams.torque_d);
            q = runTorqueQLoop(m_controlParams.torque_q);
            break;
        case ControlMode::kSpeedTorque: {
           // q = runSpeedLoop(m_controlParams.speed);
            q = std::clamp(q, -m_controlParams.max_torque, m_controlParams.max_torque);
            break;
        }
        case ControlMode::kPositionSpeedTorque: {
            float speedRef = runPositionLoop(m_controlParams.position);
            speedRef = std::clamp(speedRef, -m_controlParams.max_speed, m_controlParams.max_speed);
            //q = runSpeedLoop(speedRef);
            q = std::clamp(q, -m_controlParams.max_torque, m_controlParams.max_torque);
            break;
        }
        default:
            return;
    }

    float du, dv, dw;
    svpwm(rotorAngle, d, q, &du, &dv, &dw);
    //applyPwm(du, dv, dw); // 调用子类实现的PWM输出
}

// 角度单位转换实现
float CFOCController::degToRad(float deg) {
    return deg * kPI / 180.0f;
}

float CFOCController::radToDeg(float rad) {
    return rad * 180.0f / kPI;
}